#include "FFBWheelMain.h"
#include "class/hid/hid_device.h"
#include "tusb.h"
#include "USBCdc.h"
#include "ffb_config.h"
#include "Zy_Tools.h"
#include "Leds.h"
#include "EffectsCalculator.h"
#include "adc.h"
#include "io.h"
#include "motor.h"
#include "usb_otg.h"
#include "usbd_hid_custom_if.h"
#define DBG_TAG "ffb"
#define DBG_LVL DBG_ERROR
#include <rtdbg.h>

#define FFB_SHIFT               4
#define AXIS_TOTAL_NUM          6
#define AXIS_ADC_DEAD_ZONE      50
#define AXIS_X_MUL              22
#define AXIS_Y_MUL              22
#define AXIS_Z_MUL              22
#define AXIS_RX_MUL             22
#define AXIS_RY_MUL             22
#define AXIS_RZ_MUL             24

#define ADC_CH_AXIS_Y           0
#define ADC_CH_AXIS_RZ          1

enum
{
    AXIS_X = 0,
    AXIS_Y,
    AXIS_Z,
    AXIS_RX,
    AXIS_RY,
    AXIS_RZ
};

extern home_parameter_t homeParam[];
ffb_wheel_t ffbWheel;
static uint16_t axisReleaseValue[AXIS_TOTAL_NUM] = {0};
static const int axisMulValue[AXIS_TOTAL_NUM] =
{
    AXIS_X_MUL,
    AXIS_Y_MUL,
    AXIS_Z_MUL,
    AXIS_RX_MUL,
    AXIS_RY_MUL,
    AXIS_RZ_MUL,
};
static void FFBWheel_Adc_Get_ReleaseValue(int delayTime)
{
    rt_thread_delay(delayTime);
    axisReleaseValue[AXIS_Y] = Adc_Get_AvrValue(ADC_CH_AXIS_Y);
    axisReleaseValue[AXIS_RZ] = Adc_Get_AvrValue(ADC_CH_AXIS_RZ);

}

static int FFBWheel_Adc_Calc(int axisCh,uint16_t sample)
{
    uint16_t sub = AXIS_ADC_DEAD_ZONE+axisReleaseValue[axisCh];
    int sampleValue = sample;
    if(sampleValue < sub){
        sampleValue = sub;
    }
    sampleValue -= sub;
    sampleValue *= axisMulValue[axisCh];
    return sampleValue;
}

static void FFBWheel_Send_report(void)
{
    int32_t reportX = ffbWheel.axisX.metric.current.posScale;
    int32_t reportY = FFBWheel_Adc_Calc(AXIS_Y,Adc_Get_AvrValue(ADC_CH_AXIS_Y));
//    int32_t reportZ = 0;
//    int32_t reportRX = 0;
//    int32_t reportRY = 0;
    int32_t reportRZ = FFBWheel_Adc_Calc(AXIS_RZ,Adc_Get_AvrValue(ADC_CH_AXIS_RZ));
    CLIP(reportX,-32768,32767);
    CLIP(reportY,0,32767);
    CLIP(reportRZ,0,32767);
    // Read buttons
    Din_Read();
    Din_Get((uint8_t*)&ffbWheel.reportHID.buttons);
    ffbWheel.reportHID.buttons = ffbWheel.reportHID.buttons;
    ffbWheel.reportHID.X = reportX;
    ffbWheel.reportHID.Y = -reportY;
    ffbWheel.reportHID.Z = 0;
    ffbWheel.reportHID.RX = 0;
    ffbWheel.reportHID.RY = 0;
    ffbWheel.reportHID.RZ = -reportRZ;
    ffbWheel.reportHID.Dial = 0;
    ffbWheel.reportHID.Slider = 0;
    CUSTOM_HID_SendReport(0, (uint8_t*)(&ffbWheel.reportHID), sizeof(reportHID_t));
//    tud_hid_report(0, (uint8_t*)(&ffbWheel.reportHID), sizeof(reportHID_t));
//    ffbWheel.lastReportHID = ffbWheel.reportHID;
}



static void FFBWheel_Init(void)
{
	ffbWheel.reportHID.id = HID_ID_GAME_PAD;
	ffbWheel.usb_report_rate = 5;
	ffbWheel.axisX.axisId = 'X';
	
	Axis_Init(&ffbWheel.axisX);
	HidFFB_Init(&ffbWheel.ffb);
	EC_setDirectionEnableMask(ffbWheel.ffb.effects_calc,0x04);
	CDC_Configure_Init(&ffbWheel.axisX,&ffbWheel.ffb);

}
extern rt_sem_t sem_motorReady[];
void tud_deinit(void);
//interval_t reportIn;
void FFBWheel_Thread_Entry(void* parameter)
{
	IO_Init();
	Adc_Start();
	FFBWheel_Init();
	FFBWheel_Adc_Get_ReleaseValue(100);
	rt_sem_take(sem_motorReady[0],RT_WAITING_FOREVER);
	// 触发方向盘回零
	Canopen_Motor_Home_Start(CANOPEN_MOTOR_1_NODEID);
	// 等待电机回零完成
	rt_sem_take(homeParam[0].sem_home_success, RT_WAITING_FOREVER);
	LOG_D("ffb start");
	while(1)
	{
		if((++ffbWheel.usb_report_rate_cnt >= ffbWheel.usb_report_rate))// && tud_hid_n_ready(0))
		{
            FFBWheel_Send_report();
            ffbWheel.usb_report_rate_cnt = 0;
		}

		AxisPrepareForUpdate(&ffbWheel.axisX);
		EC_calculateEffects(ffbWheel.ffb.effects_calc,&ffbWheel.axisX,1);
		AxisUpdateDriveTorque(&ffbWheel.axisX);
		rt_thread_delay(2);
	}
}


